﻿using System;
using System.Threading;
using MLRobotic;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;

using GHIElectronics.NETMF.FEZ;

using MLRobotic.IA.Drivers;
using MLRobotic.IA.PathFinding;
using System.Collections;
using System.IO.Ports;
using System.Text;
using Devantech.Hardware.SD20;

namespace MLRobotic.IA
{
    public class Program
    {

        public static COM comPort;
      



        public static DateTime stoptime;
        public static DateTime waitime;

        public static void Init()
        {
            Debug.EnableGCMessages(false);
            Thread.Sleep(2000);
            comPort = new COM("COM1");
            //Log.Init();
            Capteurs.Init();
            IA.Init();
            AireDeJeu.Init();
            Actionneurs.Init();
            //Ecran.Init();           
         }

        public static void Main()
        {
            Robot.ishomologation = false;
            Init();
            
            #region Procedure de depart
            if (!Robot.ishomologation)
            {
                //while (IA.ProcedureDeDepart(150, 150))
                //{
                //    Actionneurs.Process();
                //    Thread.Sleep(10);
                //    Position.ProcessDirection();
                //    comPort.Process();
                //}
                if (Robot.couleur == Robot.RobotCouleur.Rouge)
                {
                    Robot.currentPosition.X = 2670;
                    Robot.currentPosition.Y = 214;
                    Robot.currentPosition.Teta = 0;
                }
                else
                {
                    Robot.currentPosition.X = 330;
                    Robot.currentPosition.Y = 214;
                    Robot.currentPosition.Teta = System.Math.PI;
                }
                Program.comPort.SetPosition(Robot.currentPosition.X, Robot.currentPosition.Y, Robot.currentPosition.Teta);
            }
            else
            {
                #region Homologation
                if (Robot.couleur == Robot.RobotCouleur.Rouge)
                {
                    Robot.currentPosition.X = 2700;
                    Robot.currentPosition.Y = 100;
                    Robot.currentPosition.Teta = System.Math.PI;
                }
                else
                {
                    Robot.currentPosition.X = 300;
                    Robot.currentPosition.Y = 100;
                    Robot.currentPosition.Teta = 0;
                }
                Program.comPort.SetPosition(Robot.currentPosition.X, Robot.currentPosition.Y, Robot.currentPosition.Teta);
                #endregion
            }
            

            #endregion

            Log.Write("PGM : Fin du recalage");
            while (Robot.Sensors.JackDepartDedans)
            {
                Thread.Sleep(10);
                Position.ProcessDirection();
                comPort.Process();
            }
            Log.Write("PGM : Go");
            stoptime = DateTime.Now.AddSeconds(90);
            //waitime = DateTime.Now.AddSeconds(3);
            while (DateTime.Now < stoptime)
            {
                //while (DateTime.Now < waitime)
                //{

                //}
                Actionneurs.Process();
                IA.Process();
                Capteurs.Process();
                Position.ProcessDirection();
                comPort.Process();
                //Ecran.Process();
            }
            Program.comPort.Stop();

        }

    }
}
